
#include "base.h"

/**
 * x，y方向联合实现获取高斯模板
 * @param Mask
 * @param wsize
 * @param sigma
 */
void generateGaussMask(Mat &Mask, Size wsize, double sigma) {

    Mask.create(wsize, CV_64F);
    int h = wsize.height;
    int w = wsize.width;
    int center_h = (h - 1) / 2;
    int center_w = (w - 1) / 2;
    double sum = 0.0;
    double x, y;
    for (int i = 0; i < h; ++i) {
        y = pow(i - center_h, 2);
        for (int j = 0; j < w; ++j) {
            x = pow(j - center_w, 2);
            //因为最后都要归一化的，常数部分可以不计算，也减少了运算量
            double g = exp(-(x + y) / (2 * sigma * sigma));
            Mask.ptr<double>(i)[j] = g;
            sum += g;
        }
    }
    Mask = Mask / sum;
}

/**
 * 按二维高斯函数实现高斯滤波
 * @param src
 * @param dst
 * @param window
 */
void GaussianFilter(Mat &src, Mat &dst, Mat window) {
    int hh = (window.rows - 1) / 2;
    int hw = (window.cols - 1) / 2;
    dst = Mat::zeros(src.size(), src.type());
    //边界填充
    Mat Newsrc;
    copyMakeBorder(src, Newsrc, hh, hh, hw, hw, BORDER_REPLICATE);//边界复制

    //高斯滤波
    for (int i = hh; i < src.rows + hh; ++i) {
        for (int j = hw; j < src.cols + hw; ++j) {
            double sum[3] = {0};

            for (int r = -hh; r <= hh; ++r) {
                for (int c = -hw; c <= hw; ++c) {
                    if (src.channels() == 1) {
                        sum[0] = sum[0] + Newsrc.at<uchar>(i + r, j + c) * window.at<double>(r + hh, c + hw);
                    }
//                    } else if (src.channels() == 3) {
//                        Vec3b rgb = Newsrc.at<Vec3b>(i + r, j + c);
//                        sum[0] = sum[0] + rgb[0] * window.at<double>(r + hh, c + hw);//B
//                        sum[1] = sum[1] + rgb[1] * window.at<double>(r + hh, c + hw);//G
//                        sum[2] = sum[2] + rgb[2] * window.at<double>(r + hh, c + hw);//R
//                    }
                }
            }

            for (int k = 0; k < src.channels(); ++k) {
                if (sum[k] < 0)
                    sum[k] = 0;
                else if (sum[k] > 255)
                    sum[k] = 255;
            }
            if (src.channels() == 1) {
                dst.at<uchar>(i - hh, j - hw) = static_cast<uchar>(sum[0]);
            }
//            } else if (src.channels() == 3) {
//                Vec3b rgb = {static_cast<uchar>(sum[0]), static_cast<uchar>(sum[1]), static_cast<uchar>(sum[2])};
//                dst.at<Vec3b>(i - hh, j - hw) = rgb;
//            }

        }
    }

}

/**
 * 分离计算实现高斯滤波，更加高效
 * @param src
 * @param dst
 * @param wsize
 * @param sigma
 */
void separateGaussianFilter(Mat &src, Mat &dst, int wsize, double sigma) {
    //获取一维高斯滤波模板
    Mat window;
    window.create(1, wsize, CV_64F);
    int center = (wsize - 1) / 2;
    double sum = 0.0;
    for (int i = 0; i < wsize; ++i) {
        double g = exp(-(pow(i - center, 2)) / (2 * sigma * sigma));
        window.at<double>(0, i) = g;
        sum += g;
    }
    window = window / sum;
    //std::cout << window << std::endl;

    //边界填充
    int boder = (wsize - 1) / 2;
    dst = Mat::zeros(src.size(), src.type());
    Mat Newsrc;
    copyMakeBorder(src, Newsrc, boder, boder, boder, boder, BORDER_REPLICATE);//边界复制

    //高斯滤波--水平方向
    for (int i = boder; i < src.rows + boder; ++i) {
        for (int j = boder; j < src.cols + boder; ++j) {
            double sum[3] = {0};

            for (int r = -boder; r <= boder; ++r) {
                if (src.channels() == 1) {
                    sum[0] = sum[0] + Newsrc.at<uchar>(i, j + r) * window.at<double>(0, r + boder); //行不变列变
                } else if (src.channels() == 3) {
                    Vec3b rgb = Newsrc.at<Vec3b>(i, j + r);
                    sum[0] = sum[0] + rgb[0] * window.at<double>(0, r + boder);//B
                    sum[1] = sum[1] + rgb[1] * window.at<double>(0, r + boder);//G
                    sum[2] = sum[2] + rgb[2] * window.at<double>(0, r + boder);//R
                }
            }
            for (int k = 0; k < src.channels(); ++k) {
                if (sum[k] < 0)
                    sum[k] = 0;
                else if (sum[k] > 255)
                    sum[k] = 255;
            }
            if (src.channels() == 1) {
                dst.at<uchar>(i - boder, j - boder) = static_cast<uchar>(sum[0]);
            } else if (src.channels() == 3) {
                Vec3b rgb = {static_cast<uchar>(sum[0]), static_cast<uchar>(sum[1]), static_cast<uchar>(sum[2])};
                dst.at<Vec3b>(i - boder, j - boder) = rgb;
            }
        }
    }

    //高斯滤波--垂直方向
    //对水平方向处理后的dst边界填充
    copyMakeBorder(dst, Newsrc, boder, boder, boder, boder, BORDER_REPLICATE);//边界复制
    for (int i = boder; i < src.rows + boder; ++i) {
        for (int j = boder; j < src.cols + boder; ++j) {
            double sum[3] = {0};

            for (int r = -boder; r <= boder; ++r) {
                if (src.channels() == 1) {
                    sum[0] = sum[0] + Newsrc.at<uchar>(i + r, j) * window.at<double>(0, r + boder); //列不变行变
                } else if (src.channels() == 3) {
                    Vec3b rgb = Newsrc.at<Vec3b>(i + r, j);
                    sum[0] = sum[0] + rgb[0] * window.at<double>(0, r + boder);//B
                    sum[1] = sum[1] + rgb[1] * window.at<double>(0, r + boder);//G
                    sum[2] = sum[2] + rgb[2] * window.at<double>(0, r + boder);//R
                }
            }
            for (int k = 0; k < src.channels(); ++k) {
                if (sum[k] < 0)
                    sum[k] = 0;
                else if (sum[k] > 255)
                    sum[k] = 255;
            }
            if (src.channels() == 1) {
                dst.at<uchar>(i - boder, j - boder) = static_cast<uchar>(sum[0]);
            } else if (src.channels() == 3) {
                Vec3b rgb = {static_cast<uchar>(sum[0]), static_cast<uchar>(sum[1]), static_cast<uchar>(sum[2])};
                dst.at<Vec3b>(i - boder, j - boder) = rgb;
            }
        }
    }
}

//获取色彩模板（值域模板）
///
void getColorMask(std::vector<double> &colorMask, double colorSigma) {

    for (int i = 0; i < 256; ++i) {
        double colordiff = exp(-(i * i) / (2 * colorSigma * colorSigma));
        colorMask.push_back(colordiff);
    }

}

//获取高斯模板（空间模板）
///
void getGaussianMask(Mat &Mask, Size wsize, double spaceSigma) {
    Mask.create(wsize, CV_64F);
    int h = wsize.height;
    int w = wsize.width;
    int center_h = (h - 1) / 2;
    int center_w = (w - 1) / 2;
    double sum = 0.0;
    double x, y;

    for (int i = 0; i < h; ++i) {
        y = pow(i - center_h, 2);
        double *Maskdate = Mask.ptr<double>(i);
        for (int j = 0; j < w; ++j) {
            x = pow(j - center_w, 2);
            double g = exp(-(x + y) / (2 * spaceSigma * spaceSigma));
            Maskdate[j] = g;
            sum += g;
        }
    }
}

/**
 * BilateralFilter
 * @param src
 * @param dst
 * @param wsize
 * @param spaceSigma
 * @param colorSigma
 */
void BilateralFilter(Mat &src, Mat &dst, Size wsize, double spaceSigma, double colorSigma) {
    Mat spaceMask;
    std::vector<double> colorMask;
    Mat Mask0 = Mat::zeros(wsize, CV_64F);
    Mat Mask1 = Mat::zeros(wsize, CV_64F);
    Mat Mask2 = Mat::zeros(wsize, CV_64F);

    getGaussianMask(spaceMask, wsize, spaceSigma);//空间模板
    getColorMask(colorMask, colorSigma);//值域模板
    int hh = (wsize.height - 1) / 2;
    int ww = (wsize.width - 1) / 2;
    dst.create(src.size(), src.type());
    //边界填充
    Mat Newsrc;
    copyMakeBorder(src, Newsrc, hh, hh, ww, ww, BORDER_REPLICATE);//边界复制;

    for (int i = hh; i < src.rows + hh; ++i) {
        for (int j = ww; j < src.cols + ww; ++j) {
            double sum[3] = {0};
            int graydiff[3] = {0};
            double space_color_sum[3] = {0.0};

            for (int r = -hh; r <= hh; ++r) {
                for (int c = -ww; c <= ww; ++c) {
                    if (src.channels() == 1) {
                        int centerPix = Newsrc.at<uchar>(i, j);
                        int pix = Newsrc.at<uchar>(i + r, j + c);
                        graydiff[0] = abs(pix - centerPix);
                        double colorWeight = colorMask[graydiff[0]];
                        Mask0.at<double>(r + hh, c + ww) = colorWeight * spaceMask.at<double>(r + hh, c + ww);//滤波模板
                        space_color_sum[0] = space_color_sum[0] + Mask0.at<double>(r + hh, c + ww);

                    } else if (src.channels() == 3) {
                        Vec3b centerPix = Newsrc.at<Vec3b>(i, j);
                        Vec3b bgr = Newsrc.at<Vec3b>(i + r, j + c);
                        graydiff[0] = abs(bgr[0] - centerPix[0]);
                        graydiff[1] = abs(bgr[1] - centerPix[1]);
                        graydiff[2] = abs(bgr[2] - centerPix[2]);
                        double colorWeight0 = colorMask[graydiff[0]];
                        double colorWeight1 = colorMask[graydiff[1]];
                        double colorWeight2 = colorMask[graydiff[2]];
                        Mask0.at<double>(r + hh, c + ww) = colorWeight0 * spaceMask.at<double>(r + hh, c + ww);//滤波模板
                        Mask1.at<double>(r + hh, c + ww) = colorWeight1 * spaceMask.at<double>(r + hh, c + ww);
                        Mask2.at<double>(r + hh, c + ww) = colorWeight2 * spaceMask.at<double>(r + hh, c + ww);
                        space_color_sum[0] = space_color_sum[0] + Mask0.at<double>(r + hh, c + ww);
                        space_color_sum[1] = space_color_sum[1] + Mask1.at<double>(r + hh, c + ww);
                        space_color_sum[2] = space_color_sum[2] + Mask2.at<double>(r + hh, c + ww);

                    }
                }
            }

            //滤波模板归一化
            if (src.channels() == 1)
                Mask0 = Mask0 / space_color_sum[0];
            else {
                Mask0 = Mask0 / space_color_sum[0];
                Mask1 = Mask1 / space_color_sum[1];
                Mask2 = Mask2 / space_color_sum[2];
            }


            for (int r = -hh; r <= hh; ++r) {
                for (int c = -ww; c <= ww; ++c) {

                    if (src.channels() == 1) {
                        sum[0] = sum[0] + Newsrc.at<uchar>(i + r, j + c) * Mask0.at<double>(r + hh, c + ww); //滤波
                    } else if (src.channels() == 3) {
                        Vec3b bgr = Newsrc.at<Vec3b>(i + r, j + c); //滤波
                        sum[0] = sum[0] + bgr[0] * Mask0.at<double>(r + hh, c + ww);//B
                        sum[1] = sum[1] + bgr[1] * Mask1.at<double>(r + hh, c + ww);//G
                        sum[2] = sum[2] + bgr[2] * Mask2.at<double>(r + hh, c + ww);//R
                    }
                }
            }

            for (int k = 0; k < src.channels(); ++k) {
                if (sum[k] < 0)
                    sum[k] = 0;
                else if (sum[k] > 255)
                    sum[k] = 255;
            }
            if (src.channels() == 1) {
                dst.at<uchar>(i - hh, j - ww) = static_cast<uchar>(sum[0]);
            } else if (src.channels() == 3) {
                Vec3b bgr = {static_cast<uchar>(sum[0]), static_cast<uchar>(sum[1]), static_cast<uchar>(sum[2])};
                dst.at<Vec3b>(i - hh, j - ww) = bgr;
            }

        }
    }

}

/**
 * 2 dimension gaussian filter
 * @param src
 * @param dst
 * @param window
 */
void unrollGaussianFilter(Mat &src, Mat &dst, Mat window) {
    int hh = (window.rows - 1) / 2;
    int hw = (window.cols - 1) / 2;
    dst = Mat::zeros(src.size(), src.type());
    //边界填充
    Mat Newsrc;
    copyMakeBorder(src, Newsrc, hh, hh, hw, hw, BORDER_REPLICATE);//边界复制
    double sum;
    //高斯滤波
    for (int i = hh; i < src.rows + hh; ++i) {
        for (int j = hw; j < src.cols + hw; ++j) {
            sum = 0;
            sum = sum + Newsrc.at<uchar>(i - hh, j - hw) * window.at<double>(0, 0);
            sum = sum + Newsrc.at<uchar>(i - hh, j - hw + 1) * window.at<double>(0, 1);
            sum = sum + Newsrc.at<uchar>(i - hh, j - hw + 2) * window.at<double>(0, 2);
            sum = sum + Newsrc.at<uchar>(i - hh + 1, j - hw) * window.at<double>(1, 0);
            sum = sum + Newsrc.at<uchar>(i - hh + 1, j - hw + 1) * window.at<double>(1, 1);
            sum = sum + Newsrc.at<uchar>(i - hh + 1, j - hw + 2) * window.at<double>(1, 2);
            sum = sum + Newsrc.at<uchar>(i - hh + 2, j - hw) * window.at<double>(2, 0);
            sum = sum + Newsrc.at<uchar>(i - hh + 2, j - hw + 1) * window.at<double>(2, 1);
            sum = sum + Newsrc.at<uchar>(i - hh + 2, j - hw + 2) * window.at<double>(2, 2);

            if (sum < 0)
                sum = 0;
            else if (sum > 255)
                sum = 255;

            dst.at<uchar>(i - hh, j - hw) = static_cast<uchar>(sum);

        }
    }
}
